#include <Wire.h>
#include <bmp.pde>
#define BMP085_ADDRESS 0x77  // I2C address of BMP085

short temperature;
long pressure;
float altitude;
float startaltitude;
float endaltitude;
float lastaltitude;
boolean missionbegin;

void setup()
{
	//Serial.begin(9600);
	Wire.begin();
	bmp085Calibration();

}

void loop()
{
	if (Serial.available())
		command=(int)Serial.read();
	if (command/10==9)
		state=command-90;
	switch(state)
	{
		case 0:	//boot, initialize some things here
			missionbegin = false;
			break;
			
		case 1: //standby
			getBMP();
			//just to initialize
			startaltitude=altitude;
			endaltitude=altitude;
			
			//compare with gps altitude
			break;
			
		case 2:	//launch
			if (command==222) 
			{
				//need a good way to be able to exit this loop
				while(abs(altitude-startaltitude)<50)
					getBMP();
				state=4;
			}
			break;
			
		case 3:	//test
			//do we put a nested switch or just run through everything possible?
		
			break;
			
		case 4:	//ascent
			//gps data
			getBMP();
			if(altitude>endaltitude)
				endaltitude=altitude;
			if(altitude<(endaltitude-50))
				state=5;
			
			break;
			
		case 5:	//release
			
			//run camera
			//gps still running
			getBMP();
			if(abs(altitude-400)<10)
			{
				//run motor
				state=6;
			}
			break;
			
		case 6:	//descent
			//keep everything running
			lastaltitude=altitude;
			getBMP();
			
			if(abs(altitude-lastaltitude)<1)
				state=7;
			break;
			
		case 7:	//impact
			//turn off everything
			//run siren
		
			break;
		default:
	}
	
	delay(1000);
}
void getBMP()
{
	temperature = bmp085GetTemperature(bmp085ReadUT());
	pressure = bmp085GetPressure(bmp085ReadUP());
	altitude = (float)44330 * (1 - pow(((float) pressure/p0), 0.190295));
}
